#!/usr/bin/env python
# -* coding: UTF-8 -*-
# @File   :main.py
# @IDE    :PyCharm
# @Author :DSF
# @Email  :19373518@buaa.edu.cn
# @Date   :2024/1/11 23:59
import yaml
import os,time
from planning_formation_lyh import link_formation,formation
import rospy
from std_msgs.msg import Int32
from geometry_msgs.msg import PoseArray



class UGVYaml:
    def __init__(self, ugv_id, waypoints, time, acc, ugv_type, v_max, w_max, kv_p, kw_p, z):
        self.ugv_id = ugv_id
        self.ugv_type = ugv_type
        self.v_max = v_max
        self.w_max = w_max
        self.Kv_p = kv_p
        self.Kw_p = kw_p
        self.waypoints = self.generate_waypoints(waypoints, time, z, acc)

    # @staticmethod
    # def time_calculate(waypoints_list, v_limit, t_start, acc):
    #     waypoints_with_time = {}
    #     y_start = waypoints_list[0][1]
    #     for item in waypoints_list:
    #         t = t_start + (item[1] - y_start) / v_limit
    #         waypoints_with_time[item] = round(t, acc)
    #     return waypoints_with_time

    @staticmethod
    def generate_waypoints(waypoints, time, z, acc):
        waypoints_list = []
        for i in range(len(waypoints)):
            pos = waypoints[i]
            pos = list(pos)
            pos.append(z)
            pos = list(map(lambda x: float(x), pos))
            pos = list(map(lambda x: round(x, acc), pos))
            t = float(time[i])
            move_type = "fixed"
            point = Waypoints(t, pos, move_type)
            waypoints_list.append(point.__dict__)
        return waypoints_list


class UAVYaml:
    def __init__(self, waypoints, time, z, acc):
        self.waypoints = self.generate_waypoints(waypoints, time, z, acc)

    # @staticmethod
    # def time_calculate(waypoints_list, v_limit, t_start, acc):
    #     waypoints_with_time = {}
    #     y_start = waypoints_list[0][1]
    #     for item in waypoints_list:
    #         t = t_start + (item[1] - y_start) / v_limit
    #         waypoints_with_time[item] = round(t, acc)
    #     return waypoints_with_time

    @staticmethod
    def generate_waypoints(waypoints, time, z, acc):
        waypoints_list = []
        for i in range(len(waypoints)):
            pos = waypoints[i]
            pos = list(pos)
            pos.append(z)
            pos = list(map(lambda x: float(x), pos))
            pos = list(map(lambda x: round(x, acc), pos))
            t = float(time[i])
            move_type = "fixed"
            point = Waypoints(t, pos, move_type)
            waypoints_list.append(point.__dict__)
        return waypoints_list


class Waypoints:
    def __init__(self, t, pos, move_type):
        self.t = t
        self.pos = pos
        self.type = move_type


def generate_yaml_ugv(ugv_id, waypoints, time, file_name, acc=1):
    yaml_ugv= UGVYaml(ugv_id, waypoints, time, acc,
                        ugv_type="unicycle", v_max=0.5, w_max=0.5, kv_p=0.5, kw_p=1.0, z=0.0)
    # print(yaml_test.__dict__)
    with open(file_name, 'w') as f:
        yaml.dump(yaml_ugv.__dict__, f)


def generate_yaml_uav(waypoints, time, z, file_name, acc=1):
    yaml_uav = UAVYaml(waypoints, time, z, acc=acc)
    # print(yaml_test.__dict__)
    with open(file_name, 'w') as f:
        yaml.dump(yaml_uav.__dict__, f)


def obs_callback(msg):
    global obs_list
    obs_list = []
    a = []
    n = len(msg.poses)
    for i in range(n):
        a = [msg.poses[i].position.x*10 + 3,
             msg.poses[i].position.y*10 + 3,
             msg.poses[i].position.z*10 + 3]
        # print(a)
        obs_list.append(a)

if __name__ == "__main__":

    obs_list=[]
    rospy.init_node('main', anonymous=True)
    rospy.Subscriber('/obstacles', PoseArray, obs_callback, queue_size=10)
    
    time.sleep(1)
    print(obs_list)

    # Field_width = 100,  # 短边边长 单位分米
    # Field_length = 400,  # 长边边长 单位分米
    # obstacle_num = 5,  # 障碍物数量，有动捕时无效
    # obstacle = None,  # 动捕障碍物信息格式[[x,y,r]...[x,y,r]]
    # radius = 10,  # 障碍物半径 半径分米
    # speed = 2,  # 速度 2分米每秒 0.2米每秒
    # speed_limit = 12,  # ugv速度上限，1.5米每秒,即15 dm/s
    # formation_times = 3,  # 3秒形成编队
    # ugv_num = 4,  # 默认四车编队
    # edge_length = 15,  # 编队距中心点距离,默认为 15 dm
    # loss_ugv = None,  # 该编号节点丢失,只支持单节点丢失，如果设置此处，formation_type必须含有编队丢失信息
    # formation_change_time = None,  # 编队变换时间列表,必须与formation_type数目匹配！！
    # formation_type = None,  # 优先级最高！编队在该时间点的类型设定,3、4、5....N为正N边形，尖头为超前方向，2为正方形方头朝前，1为一字型编队,数字跨越则不生效
    # startPointList = None,  # 编队起点终点设置,从动补中获得,第一个车默认为主车
    # formation_startPoint = None,  # 编队开始时主车坐标 默认开始时为菱形
    # endPoint = None,  # 主车终点设置
    # baseLine = None,
    # Map_type = None  # 地图类型"MotionCapture" or "UWB"
    path_dict = link_formation(obstacle = obs_list,
                          Field_length = 100,
                          Field_width = 100, 
                          ugv_num = 1,
                        #   加速到0.4m/s
                          speed = 4,
                          edge_length = 9,
                          formation_change_time = [1, 80, 150],
                          keyPointList=[(-10, 10), (30, 40), (-30, 40)],
                        #   startPointList = [[-10, -50]],
                          formation_startPoint= [-10, -40],
                          Map_type="MotionCapture",
                          Add_circle = False,
                        #   loss_ugv = 'random',
                          isDrawMap = True,
                        )
    # 预备加一个发布节点丢失位置的功能
    stop_point = (3.0,4.0)
    stop_time = 29
    stop_flag = 0
    for key in path_dict:
        print("键值：",key)
        print("内容:",path_dict[key])
        temp_list1 = []
        temp_list2 = []
        length = len(path_dict[key][0])
        print("当前列表为:",path_dict[key][0])
        for i in range(length):
            temp_list1.append(path_dict[key][0][i])
            temp_list2.append(path_dict[key][1][i])
        for i in range(length):
            if path_dict[key][0][i] == stop_point and stop_flag == 0:
                print("出现停止点",path_dict[key][0][i],i)
                stop_flag = 1
                for j in range(i + 1,length):
                    temp_list2[j] += stop_time
                temp_list1.insert(i + 1,stop_point)
                temp_list2.insert(i + 1,path_dict[key][1][i] + stop_time)
            print("当前列表为:",path_dict[key][0])
        path_dict[key][0] = temp_list1
        path_dict[key][1] = temp_list2
        print("加入停止点之后的内容:",path_dict[key])
    # """
    # 无人车yaml文件生成demo
    # """
    # for key, value in path_dict.items():
    #     folder = os.path.dirname(os.path.abspath(__file__))
    #     folder = folder + '/config/ugv_waypoints_' + str(key+1) + '.yaml'
    #     # 编辑存储yaml文件的位置和名称
    #     generate_yaml_ugv(key+1, value[0], value[1], folder)

    """
    无人机yaml文件生成demo
    """
    key_set = 0
    for key, value in path_dict.items():
        if key == key_set:
            folder = os.path.dirname(os.path.abspath(__file__))
            folder = folder + '/config/uav_waypoints_' + str(key) + '.yaml'
            # 编辑存储yaml文件的位置和名称
            generate_yaml_uav(value[0], value[1], 5.0, folder)
    
    
